#include "Location3dOperation.h"


 namespace kybot_map 
 {
    Location3dOperation::Location3dOperation()
    {
      initial3DPosePuber_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1, false);
      mapPuber_ = nh_.advertise<std_msgs::String>("/pcdmap_update", 1, false);
    }
    bool Location3dOperation::RelocatorPos(geometry_msgs::Pose2D pose)
    {
        geometry_msgs::PoseWithCovarianceStamped poseWithCovarianceStamped;
        poseWithCovarianceStamped.header.frame_id = "map";
        poseWithCovarianceStamped.header.stamp = ros::Time::now();
        poseWithCovarianceStamped.pose.pose.position.x = pose.x + 0.472 * cos(pose.theta);//坐标补偿
        poseWithCovarianceStamped.pose.pose.position.y = pose.y + 0.472 * sin(pose.theta);
        poseWithCovarianceStamped.pose.pose.position.z = 0.0;
        poseWithCovarianceStamped.pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose.theta);
        poseWithCovarianceStamped.pose.covariance[0] = 0.01;  //x-x
        poseWithCovarianceStamped.pose.covariance[7] = 0.01;  //y-y
        poseWithCovarianceStamped.pose.covariance[35] = 0.01; //theta-theta
        initial3DPosePuber_.publish(poseWithCovarianceStamped);
        return true;

    }
    bool Location3dOperation::StartLocalization(const std::string& param)
    {

       return true;
    }
    bool Location3dOperation::StopLocalization(const std::string& param)
    {
        return true;
    }
    bool Location3dOperation::SwitchMap(const std::string& map_name)
    {
         std_msgs::String msg;
         msg.data = map_name;
         mapPuber_.publish(msg);
    }
    bool Location3dOperation::SearchMap(std::vector<std::string>& mapNameList_,const std::string& mapFolderPath_)
    {
        // 清空输入列表
        ROS_INFO("mapFolderPath_:%s",mapFolderPath_.c_str());
        mapNameList_.clear();
        // 检查文件夹路径是否有效
        if (mapFolderPath_.empty()) {
            std::cerr << "Error: Map folder path is not set." << std::endl;
            return false;
        }
        try {
            // 使用 boost::filesystem 遍历目录
            boost::filesystem::path dirPath(mapFolderPath_);
            
            if (!boost::filesystem::exists(dirPath) || !boost::filesystem::is_directory(dirPath)) {
                std::cerr << "Error: Invalid directory path: " << mapFolderPath_ << std::endl;
                return false;
            }
            // 遍历目录中的所有文件
            for (auto& entry : boost::filesystem::directory_iterator(dirPath)) {
                if (boost::filesystem::is_regular_file(entry)) {
                    std::string fileName = entry.path().filename().string();
                    std::string extension = entry.path().extension().string();
                    
                    // 转换为小写并检查扩展名是否为 .pcd
                    std::transform(extension.begin(), extension.end(), extension.begin(), 
                                [](unsigned char c) { return std::tolower(c); });
                    
                    if (extension == ".pcd") {
                        mapNameList_.push_back(fileName);
                    }
                }
            }
        }
        catch (const boost::filesystem::filesystem_error& e) {
            std::cerr << "Filesystem error: " << e.what() << std::endl;
            return false;
        }
        catch (const std::exception& e) {
            std::cerr << "Error searching map folder: " << e.what() << std::endl;
            return false;
        }
        // 输出找到的文件数量
        std::cout << "Found " << mapNameList_.size() << " .pcd files in " << mapFolderPath_ << std::endl;
        return true;
    }
}